#include "common_def.h"
#include "debug_print.h"
#include <ctype.h>

uint16 str_to_uint16(const uint8* str)
{
  uint16 result = 0;
  for (;;)
  {
    uint8 c = *str++;
    if (isdigit(c))
      result = result * 10 + c - '0';
    else
      break;
  }
  return result;
}

uint32 str_to_uint32(const uint8* str)
{
  uint32 result = 0;
  for (;;)
  {
    uint8 c = *str++;
    if (isdigit(c))
      result = result * 10 + c - '0';
    else
      break;
  }
  return result;
}

float str_to_float(const uint8* str)
{
  bool minus = false;
  if (*str == '-')
  {
    minus = true;
    str++;
  }
  float result = (float)str_to_uint32(str);
  while (isdigit(*str))
    str++;
  float factor = 0.1;
  if (*str++ == '.')
  {
    while (isdigit(*str))
    {
      result += factor * (float)(*str++ - '0');
      factor /= 10.0;
    }
  }
  if (minus)
  {
    result = -result;
  }
  return result;
}

//uint8 int_to_str(uint8 val, uint8* buffer)
//{
//  // max value of 16 bit int is 65535, 5 digits is enough
//  uint8 buf[5];
//  uint8 i = 0;
//  uint8 result = 0;
//
//  do
//  {
//    uint8 v = val % 10 + '0';
//    val /= 10;
//    buf[i++] = v;
//  } while (val > 0);
//  while (i > 0)
//    buffer[result++] = (buf[--i]);
//  return result;
//}

uint8 bcd_decode(uint8 data)
{
  return ((data) & 0x0f) + ((data) >>4) * 10;
}

uint8 bcd_encode(uint8 data)
{
  return ((data / 10) << 4) | (data % 10);
}

// memory allocation
#define POOL_SIZE (1024*9)
struct MEMORY_BLOCK_T {
  uint16 used;
  uint16 size;
};
//static uint8 memory_pool[POOL_SIZE];

#pragma data_alignment=4
static __no_init uint8 memory_pool[POOL_SIZE];

#define BLOCK_HEADER_SIZE sizeof(struct MEMORY_BLOCK_T)
#define BLOCK_AT(offset) ((struct MEMORY_BLOCK_T*)(&memory_pool[offset]))

void mem_pool_init(void)
{
  struct MEMORY_BLOCK_T* block = BLOCK_AT(0);
  block->used = 0;
  block->size = POOL_SIZE;
}

void mem_pool_dump(void)
{
  uint16 offset = 0;
  while (offset < POOL_SIZE)
  {
    struct MEMORY_BLOCK_T* block = BLOCK_AT(offset);
    debug_printf((const uint8*)"Block at offset %d, ptr=%x, size %d, used %d\n", offset, block, block->size, block->used);
    offset += block->size;
  }
}

void mem_pool_check(void)
{
  uint16 offset = 0;
  while (offset < POOL_SIZE)
  {
    struct MEMORY_BLOCK_T* block = BLOCK_AT(offset);
    ASSERT(block->size >= 4);
    ASSERT(block->size <= POOL_SIZE);
    ASSERT(block->used == 0 || block->used == 1);
    offset += block->size;
  }
}

void* mem_alloc(uint16 size)
{
  // search all blocks
  uint16 offset = 0;
  uint8* result = NULL;
  size = (size + 3) & ~3;

  while (offset < POOL_SIZE) {
    struct MEMORY_BLOCK_T* block = BLOCK_AT(offset);
    ASSERT(block->size >= 4);
    ASSERT(block->size <= POOL_SIZE);
    if ((block->size >= size + BLOCK_HEADER_SIZE) && (block->used == 0)) {
      if (block->size > size + BLOCK_HEADER_SIZE * 2 + 4) {
          // this block is available, and bigger than required: split
          uint16 unused = offset + BLOCK_HEADER_SIZE + size;
          struct MEMORY_BLOCK_T* new_block = BLOCK_AT(unused);
          // set new block
          new_block->size = block->size - BLOCK_HEADER_SIZE - size;
          new_block->used = 0;
          // mark block used
          block->size = BLOCK_HEADER_SIZE + size;
          block->used = 1;
          result = (uint8*)&block[1];
          memset(result, 0, size);
          return result;
      } else {
        // this block is just as big as required, return it directly
        block->used = 1;
        result = (uint8*)&block[1];
        memset(result, 0, size);
        return result;
      }
    } else {
      offset += block->size;
    }
  }
  debug_printf((const uint8*)"Failed to alloc %d bytes. Dumping memory pool\n", size);
  mem_pool_dump();
  RESET_DEVICE();
  return NULL;
}

void mem_free(void* ptr)
{
  uint16 block_offset = (uint8*)ptr - memory_pool;
  struct MEMORY_BLOCK_T* block;
  uint16 offset = 0;

  if (block_offset > POOL_SIZE) {
    debug_printf((const uint8*)"Freeing invalid memory\n");
    mem_pool_dump();
    RESET_DEVICE();
  } else {
    block_offset -= BLOCK_HEADER_SIZE;
    block = BLOCK_AT(block_offset);
    block->used = 0;
    // scan and merge
    offset = 0;
    while (offset < POOL_SIZE) {
      block = BLOCK_AT(offset);
      if (block->used == 0) {
        uint16 next_offset;
        struct MEMORY_BLOCK_T* next_block;
        for (next_offset = offset + block->size;
             (next_offset < POOL_SIZE) && (next_block = BLOCK_AT(next_offset), next_block->used == 0);
             next_offset += next_block->size) {
               // merging
               if (next_block->size == 0) {
                 ASSERT(next_block->size != 0);
               }
               block->size += next_block->size;
             }
      }
      offset += block->size;
    }
  }
}

uint8 s_high_freq_modules = 0;

void power_enable_high_freq(enum HIGH_FREQ_MODULE module)
{
  s_high_freq_modules |= (uint8)module;
}

void power_disable_high_freq(enum HIGH_FREQ_MODULE module)
{
  s_high_freq_modules &= ~(uint8)module;
}

void power_go_sleep(void)
{
  if (s_high_freq_modules == 0)
  {
    ENTER_LPM3();
  }
  else
  {
    ENTER_LPM0();
  }
}
